﻿using System.Collections;
using System.Collections.Generic;

namespace TrueSync
{
    public struct TSTransform
    {
        public TSVector2 p;
        public Rot q;
        public bool mirror;

        /// <summary>
        /// Initialize using a position vector and a rotation matrix.
        /// </summary>
        /// <param name="position">The position.</param>
        /// <param name="rotation">The r.</param>
        public TSTransform(TSVector2 position, Rot rotation, bool mirror)
        {
            p = position;
            q = rotation;
            this.mirror = mirror;
        }

        /// <summary>
        /// Set this to the identity transform.
        /// </summary>
        public void SetIdentity()
        {
            p = TSVector2.zero;
            q.SetIdentity();
            mirror = false;
        }

        public TSVector2 TransformPoint(TSVector2 v)
        {
            return TSVector2.Multiply((p + Rot.Mul(q, v)), new TSVector2(mirror ? -1 : 1, 1));
        }

        public TSVector2 TransformVector(TSVector2 v)
        {
            return TSVector2.Multiply(Rot.Mul(q, v), new TSVector2(mirror ? -1 : 1, 1));
        }

        public TSVector2 InverseTransformPoint(TSVector2 v)
        {
            v = TSVector2.Multiply(v, new TSVector2(mirror ? -1 : 1, 1));
            FP px = v.x - p.x;
            FP py = v.y - p.y;
            FP x = (q.c * px + q.s * py);
            FP y = (-q.s * px + q.c * py);

            return new TSVector2(x, y);
        }

        public TSVector2 InverseTransformVector(TSVector2 v)
        {
            v = TSVector2.Multiply(v, new TSVector2(mirror ? -1 : 1, 1));
            return Rot.MulT(q, v);
        }

        public static TSTransform Mul(TSTransform A, TSTransform B)
        {
            TSTransform C = new TSTransform();
            C.q = Rot.Mul(A.q, B.q);
            C.p = Rot.Mul(A.q, B.p) + A.p;
            C.mirror = A.mirror ^ B.mirror;
            return C;
        }
    }
}

